#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <omp.h>

struct pointer {
	struct node *ptr;
	int count;
};

struct node {
	struct pointer *next;
	void *value;
};

struct queue {
	struct pointer *head;
	struct pointer *tail;
};

void initialize( struct queue *queue) {
	struct node *node;
	struct pointer *next;
	struct pointer *headpointer;
	struct pointer *tailpointer;

	// prepare node
	node = malloc( sizeof(struct node) );
	next = malloc( sizeof(struct pointer) );
	node->value = NULL;
	node->next = next;
	next->ptr = NULL;
	next->count = 0;

	headpointer = malloc( sizeof(struct pointer) );
	tailpointer = malloc( sizeof(struct pointer) );
	headpointer->ptr = node;
	tailpointer->ptr = node;
	headpointer->count = 0;
	tailpointer->count = 0;
	queue->head = headpointer;
	queue->tail = tailpointer;
}

struct pointer* newPointer(struct node *node, int count) {
	struct pointer *pointer;
	pointer = malloc(sizeof(struct pointer));
	pointer->ptr = node;
	pointer->count = count;
	return pointer;
}

// append data to q->tail
void enqueue( struct queue *queue, void *value ) {
	struct node *node;
	struct pointer *nxt;
	struct pointer *tail;
	struct pointer *next;

	// prepare new node
	node = malloc( sizeof(struct node) );
	nxt = malloc( sizeof(struct pointer) );
	node->value = value;
	node->next = nxt;
	nxt->ptr = NULL;
	nxt->count = 0;

	while(1) {
		tail = queue->tail;
		next = tail->ptr->next;
		if((tail->count == queue->tail->count) && (tail->ptr == queue->tail->ptr)) {
			if (next->ptr == NULL) {
				if (__sync_bool_compare_and_swap (&tail->ptr->next, next, newPointer(node, next->count+1))) {
					break;
				}
			}
			else {
				__sync_bool_compare_and_swap (&queue->tail, tail, newPointer(next->ptr, tail->count+1));
			}
		}
	}
	__sync_bool_compare_and_swap (&queue->tail, tail, newPointer(node, tail->count+1));
}

// return q->head->data
void *dequeue( struct queue *queue) {
	void *pvalue;
	struct pointer *head;
	struct pointer *tail;
	struct pointer *next;
	int shouldBreak = 0;

	while(1) {
		head = queue->head;
		tail = queue->tail;
		next = head->ptr->next;
		if((head->count == queue->head->count) && (head->ptr == queue->head->ptr)) {
			if (head->ptr == tail->ptr) {
				if (next->ptr == NULL) {
					return NULL;
				}
				__sync_bool_compare_and_swap (&queue->tail, tail, newPointer(next->ptr, tail->count+1));
			}
			else {
				pvalue = next->ptr->value;

				if (__sync_bool_compare_and_swap (&queue->head, head, newPointer(next->ptr, head->count+1))) {
					break;
				}
			}
		}
	}
	free(head->ptr);
	return pvalue;
}


int main( int argc, char **argv ) {
	struct queue *q;
	q = malloc( sizeof(struct queue) );

	initialize(q);

	#pragma omp parallel 
	#pragma omp single nowait 
	{
		long i=0;
		for(i=1;i<3;++i) {
			#pragma omp task 
			{
				long j=0;
				for(j=0;j<20;++j) {
					enqueue(q,(void*)(i*1000+j));
				}
			}
			#pragma omp task 
			{
				long d;
				long j=0;
				for(j=0;j<20;++j) {
					d = (long)dequeue(q);
					if(d)
						printf("dequeue %ld\n",d);
				}
			}
		}
	}

	long d;
	while(1) {
		d = (long)dequeue(q);
		if(!d)
			break;
		printf("dequeue outside %ld\n",d);
	}
	assert(q->head->ptr == q->tail->ptr);
	return 0;
}
